#!/usr/bin/env python
#
# *********     Ping Example      *********
#
#
# Available SCServo model on this example : All models using Protocol SCS
# This example is tested with a SCServo(STS/SMS/SCS), and an URT
# Be sure that SCServo(STS/SMS/SCS) properties are already set as %% ID : 1 / Baudnum : 6 (Baudrate : 1000000)
#

from ftauto.scservo_sdk import *                 # Uses SCServo SDK library
import os
import sys, tty, termios
from flask import jsonify
# fd = sys.stdin.fileno()
# old_settings = termios.tcgetattr(fd)
# def getch():
    # try:
        # tty.setraw(sys.stdin.fileno())
        # ch = sys.stdin.read(1)
    # finally:
        # termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
    # return ch

def scanID(ID=None):    
    SCS_ID                  = int(ID)                 # SCServo ID : 1
    BAUDRATE                = 1000000           # SCServo default baudrate : 1000000
    DEVICENAME              = '/dev/ttyUSB0'    # Check which port is being used on your controller
    protocol_end            = 0                 # SCServo bit end(STS/SMS=0, SCS=1)

    # Initialize PortHandler instance
    # Set the port path
    # Get methods and members of PortHandlerLinux or PortHandlerWindows
    portHandler = PortHandler(DEVICENAME)

    # Initialize PacketHandler instance
    # Get methods and members of Protocol
    packetHandler = PacketHandler(protocol_end)
    # Open port
    if portHandler.openPort() and portHandler.setBaudRate(BAUDRATE):
        flag = 1       
    else:
        flag = -1   

    # Try to ping the SCServo
    # Get SCServo model number
    scs_model_number, scs_comm_result, scs_error = packetHandler.ping(portHandler, SCS_ID)
    if scs_comm_result == COMM_SUCCESS:
       flag = 1
    # Close port
    portHandler.closePort()
    return jsonify({'ID':ID,'result':scs_model_number})